#include "PID.h"

/******************************************************
 * Update PID controller using current error and position.
 * Code modeled after Embedded.com article.
 ******************************************************/
long PIDUpdate(SPid* pid, long error, long derivative)
{
	long pTerm, iTerm, dTerm;
	
	// Calculate the proportional term
	pTerm = (pid->pGain * error) >> 8;
	
	// Calculate the integral state with limiting
	pid->iState += error;
	
	// Unwind the integrator
	if (pid->iState > pid->iMax)
	{
		pid->iState = pid->iMax;
	}
	else if (pid->iState < pid->iMin)
	{
		pid->iState = pid->iMin;
	}
	
	// Calculate the integral term
	iTerm = (pid->iGain * pid->iState) >> 8;
	
	// Calculate the derivative term
	dTerm = (pid->dGain * (derivative)) >> 8;
	
	// Update derivative state
	pid->dState = derivative;
	
	// Return the whole controller
	return pTerm + iTerm - dTerm;
}
